// Copyright (c) 2012, Hazen Eckert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the The University of Texas at Dallas nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT DALLAS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "auv_driver_common/Serial.h"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include <string.h>
#include <ros/ros.h>

#define STX 0x02
#define ETX 0x03

using namespace std;

#include "auv_driver_common/auv_driver.h"

auv_driver::auv_driver()
{
    driverState = CLOSED;
}

auv_driver::~auv_driver()
{
    if (driverState == OPENED)
        serial->end();
    delete serial;
}

void auv_driver::open( const char *port, long baud )
{
    if (driverState == OPENED) {
        // warning
        ROS_ERROR("already opened. closing...");
        serial->end();
        delete serial;
        driverState = auv_driver::CLOSED;
    }
    try {   
    	ROS_ERROR("opening %s", port);
			serial = new Serial(port);
    } catch (int e) {
    	driverState = auv_driver::CLOSED;
    	if (e == 19)
    		throw 19;
    }
    if (serial == NULL) {
        // error
        ROS_ERROR("serial is null");
        driverState = auv_driver::CLOSED;
    } else {
    		ROS_ERROR("serial has begun");
        serial->begin(baud);
        driverState = auv_driver::OPENED;
    }
    
}

void auv_driver::close()
{
    if (driverState == OPENED) {
    		ROS_ERROR("closing..");
        serial->end();
    } else {
    		ROS_ERROR("ALready closed");
        // warning
    }
    driverState = auv_driver::CLOSED;
}

int auv_driver::getProcessorType( uint8_t* family, uint8_t* specific )
{
    if (driverState == CLOSED) {
        // error
        ROS_ERROR("cannot send to closed device");
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x01;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL)  {
    	ROS_ERROR("no response");
    	return -1;
    }
    // error checking
    
    *family = *(uint8_t *)(packet->data);
    *specific = *(uint8_t *)(packet->data+1);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::getSoftwareVersion( uint16_t *major, uint16_t *minor, uint16_t *incremental )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x02;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *major = *(uint16_t *)(packet->data);
    *minor = *(uint16_t *)(packet->data+2);
    *incremental = *(uint16_t *)(packet->data+4);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::getProcessorDescription( char **stringBuffer )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x03;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *stringBuffer = (char *)malloc(packet->length+1);
    bzero(*stringBuffer, packet->length+1);
    memcpy(*stringBuffer, packet->data, packet->length);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}


int auv_driver::getMaxPayload( uint8_t *requestBufferSize, uint8_t *responseBufferSize )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x04;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *requestBufferSize = *(uint8_t *)(packet->data);
    *responseBufferSize = *(uint8_t *)(packet->data+1);
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::getBoardFunction( uint8_t *boardFunction )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x05;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *boardFunction = *(uint8_t *)(packet->data);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}
	
int auv_driver::getBoardSerialNumber( char **stringBuffer )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x06;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *stringBuffer = (char *)malloc(packet->length+1);
    bzero(*stringBuffer, packet->length+1);
    memcpy(*stringBuffer, packet->data, packet->length);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}	

int auv_driver::getBoardDescription( char **stringBuffer )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x07;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *stringBuffer = (char *)malloc(packet->length+1);
    bzero(*stringBuffer, packet->length+1);
    memcpy(*stringBuffer, packet->data, packet->length);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::getBoardStatus( uint8_t *boardStatus )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x08;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL) 
    	return -1;
    // error checking
    
    *boardStatus = *(uint8_t *)(packet->data);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::getBoardVoltage( float *boardVoltage )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x10;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL)
    	return -1;
    // error checking
    
    *boardVoltage = *(float *)(packet->data);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::getBoardCurrent( float *boardCurrent )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x11;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL)
    	return -1;
    // error checking
    
    *boardCurrent = *(float *)(packet->data);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}

int auv_driver::resetCommunication( )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x12;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL)
    	return -1;
    // error checking
    
    delete packet;
    
    return 0;
}

int auv_driver::resetMicrocontroller( )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x13;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = NULL;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL)
    	return -1;
    // error checking
    
    delete packet;
    
    return 0;
}

int auv_driver::storeStatus( uint8_t options, uint8_t *storageType, uint8_t *status )
{
    if (driverState == CLOSED) {
        // error
        return -1;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket;
    
    packet->type = 0x11;
    packet->flags = 0x00;
    packet->length = 0x00;
    packet->data = new uint8_t;
    
    *packet->data = options;
    
    sendICDpacket(packet);
    
    delete packet;
    
    packet = recieveICDpacket();
    if (packet == NULL)
    	return -1;
    // error checking
    
    *storageType = *(uint8_t *)(packet->data);
    *status = *(uint8_t *)(packet->data + 1);
    
    free(packet->data);
    
    delete packet;
    
    return 0;
}


bool auv_driver::isOpened()
{
    if (driverState == CLOSED) {
        return false;
    }
    
    return true;
}

void auv_driver::sendICDpacket( auv_driver::ICDpacket *packet )
{
    char buffer[255];
    buffer[0] = STX;
    buffer[1] = packet->type;//TYP
    buffer[2] = packet->flags;//FLG
    buffer[3] = packet->length;//LEN
    memcpy(buffer+4, packet->data, packet->length);
    buffer[4+packet->length] = checksum(buffer, 4+packet->length);
    buffer[5+packet->length] = ETX;
    serial->tx(buffer, 6+packet->length);
}

auv_driver::ICDpacket * auv_driver::recieveICDpacket()
{
    char buffer[255];
    int n = serial->rx(buffer, 255);
    
    if (n < 6) {
    	n+= serial->rx(buffer+n, 255);
    }
    
    if (buffer[0] != STX) {
    		ROS_ERROR("invalid header");
        return NULL;
    }
    auv_driver::ICDpacket *packet = new auv_driver::ICDpacket();
    packet->type = buffer[1];
    packet->flags = buffer[2];
    packet->length = buffer[3];
    
    int attempts = 0;
    while ( n < 6 + packet->length && attempts < 5) { // add timeout
    		n+= serial->rx(buffer+n, 255);
            usleep(100);
            attempts++;
    }
    
    packet->data = (uint8_t *)malloc(packet->length);
    memcpy(packet->data, buffer+4, packet->length);
    
   	if ( (uint8_t)buffer[4+packet->length] != checksum(buffer, 4+packet->length)) {
    		free(packet->data);
    		delete packet;
        return NULL;
    }
    
    if ((buffer[5+packet->length] != ETX)) {
    		free(packet->data);
    		delete packet;
    		ROS_ERROR("invalid ETX");
        return NULL;
    }
    
    return packet;
}

uint8_t auv_driver::checksum( char *buffer, int length )
{
	uint8_t sum = 0;
	for(int i = 0; i < length; i++)
	{
		sum += buffer[i];
	}
	return sum;
}

